/**
 * Snowflake MEGA sumo robot main program
 *
 * Written for qualifying All Japan Robot-Sumo Tournament 15.12.2013.
 *  - already WON RobotChallenge in Vienna 23-24.03.2013
 *  - didn't participate RoboGames in San Mateo 19-21.04.2013
 *  - will WIN Robotex in Tallinn 16-17.11.2013
 *
 * Copyright 2013 Maris BECS <maris.becs@gmail.com>
 */

#include <asf.h>
#include <string.h>
#include "main.h"
#include "unit_tests.h"
#include "strategies.h"
#include "keyboard.h"
#include "bitmaps.h"
#include "sensors.h"
#include "motors.h"


/* Main menu: strings are stored in FLASH,
 * the string pointer table and menu struct are stored in RAM.
 */
PROGMEM_DECLARE(char, main_menu_title[]) = "      -Main Menu-";
PROGMEM_DECLARE(char, main_menu_1[]) = "FIGHT";
PROGMEM_DECLARE(char, main_menu_2[]) = "Tests";
PROGMEM_DECLARE(char, main_menu_3[]) = "Servo:    Left/Right";

PROGMEM_STRING_T main_menu_strings[] = {
	main_menu_1,
	main_menu_2,
	main_menu_3
};

struct gfx_mono_menu main_menu = {
	// Title
	main_menu_title,
	// Array with menu strings
	main_menu_strings,
	// Number of menu elements
	3,
	// Initial selection
	0
};

void button_splash(void);

/*static void global_callback(void)
{
    // User code to execute when the overflow occurs here
    //Speed=100;
    // Toggle LCD Backlight
    //gpio_toggle_pin(NHD_C12832A1Z_BACKLIGHT);
    //tc_disable(&TCC0);	//not needed any more
}*/


/**
 * \brief Main function.
 * Initializes the robot board and then runs the main menu system in an infinite while loop.
 */
int main(void)
{
	Robot_state = RS_NONE;
	PWM_L=0;
	PWM_R=0;
	Speed=0;
	Time_ms=1000;	//1 sec
	servo_pos = false;

	uint8_t menu_status;
	struct keyboard_event input;

	sysclk_init();
	pmic_init();
	board_init();

    /*/ not tested on the robot
    tc_enable(&TCC0);	// 16-bit => max=65536
    tc_set_overflow_interrupt_callback(&TCC0, global_callback);
    tc_set_wgm(&TCC0, TC_WG_NORMAL);
    tc_write_period(&TCC0, 62500);	// 0.5Hz => 2sec delay at the line
    tc_set_overflow_interrupt_level(&TCC0, TC_INT_LVL_LO);
    tc_write_clock_source(&TCC0, TC_CLKSEL_DIV1024_gc);	// 32MHz/1024= 31250 Hz
    tc_reset(&TCC0);*/

	// init IR module to OFF state
	ioport_set_pin_high(START_SW);

	#ifdef DEBUG_ENABLE
		// USART options
		static usart_serial_options_t usart_options = {
			.baudrate = USART_SERIAL_BAUDRATE,
			.charlength = USART_SERIAL_CHAR_LENGTH,
			.paritytype = USART_SERIAL_PARITY,
			.stopbits = USART_SERIAL_STOP_BIT
		};
		usart_serial_init(USART_SERIAL, &usart_options);
	#endif

	// Not sure about proper place of _init() functions
	motors_init();

	gfx_mono_init();	// Initialize GFX lib. Will configure the display controller and create a frame buffer in RAM.

	rtc_init();
	// Enable interrupts
	cpu_irq_enable();

	#ifdef DEBUG_ENABLE
		// Send CPU clock in MHz
		usart_serial_putchar( USART_SERIAL, F_CPU / 1000000 );	// 32 expected
	#endif

	// Enable display back-light
	gpio_set_pin_high(NHD_C12832A1Z_BACKLIGHT);

	// Draw Logo and wait for any key press&release to exit
	gfx_mono_put_bitmap(&bitmap_snowflake, 0, 0);
	// Any key release will exit the loop
	while (true) {
		keyboard_get_key_state(&input);
		if (input.type == KEYBOARD_RELEASE) {
			break;
		}
	}

	// put servo in initial position
	turn_servo(false);
	delay_ms(200);
	pwm_start( &pwm_cfg[1], 0 );


	/* Main loop. Read keyboard status and pass input to menu system.
	 * When an element has been selected in the menu, it will return the index of
	 * the element that should be run. This can be an application or another menu.
	 */
	while (true) {

		// (re)initialize menu system
		gfx_mono_menu_init(&main_menu);

		do {
			do {
				keyboard_get_key_state(&input);
			// Wait for key release
			} while (KEYBOARD_RELEASE != input.type);

			// Send key to menu system
			menu_status = gfx_mono_menu_process_key(&main_menu, input.keycode);
		// Wait for something useful to happen in the menu system
		} while (menu_status == GFX_MONO_MENU_EVENT_IDLE);

		switch (menu_status) {
		case 0:
			strategies_application();
			break;
		case 1:
			unit_tests_application();
			break;
		case 2:
			// Toggle LCD Backlight
			//gpio_toggle_pin(NHD_C12832A1Z_BACKLIGHT);

			// switch servo position
			if (servo_pos) {
				turn_servo(false);
				servo_pos = false;
			} else {
				turn_servo(true);
				servo_pos = true;
			}
			delay_ms(200);
			pwm_start( &pwm_cfg[1], 0 );
			break;

		case GFX_MONO_MENU_EVENT_EXIT:
			// Fall trough to default and show button splash
		default:
			// Display a splash screen showing button functions
			button_splash();
			break;
		};
	}
}


/**
 * \brief Show button names on display
 */
void button_splash(void)
{
	struct keyboard_event input;

	// put servo in initial position
	turn_servo(false);
	delay_ms(200);
	pwm_start( &pwm_cfg[1], 0 );

	// Clear screen
	gfx_mono_draw_filled_rect(0, 0, 128, 32, GFX_PIXEL_CLR);

	// Draw button position representing circles
	gfx_mono_draw_filled_circle(10, 6, 4, GFX_PIXEL_SET, GFX_WHOLE);
	gfx_mono_draw_filled_circle(92, 6, 4, GFX_PIXEL_SET, GFX_WHOLE);
	gfx_mono_draw_circle(33, 6, 4, GFX_PIXEL_SET, GFX_WHOLE);
	gfx_mono_draw_filled_circle(122, 6, 4, GFX_PIXEL_SET, GFX_WHOLE);
	gfx_mono_draw_filled_circle(122, 26, 4, GFX_PIXEL_SET, GFX_WHOLE);

	// Draw button names
	gfx_mono_draw_string("Up", 104, 3, &sysfont);
	gfx_mono_draw_string("Down", 91, 22, &sysfont);
	gfx_mono_draw_string("Enter", 55, 3, &sysfont);
	gfx_mono_draw_string("Back", 0, 15, &sysfont);

	// Any key release will exit the loop
	while (true) {
		keyboard_get_key_state(&input);
		if (input.type == KEYBOARD_RELEASE) {
			break;
		}
	}
}